#include <avr/interrupt.h>
#include "timer.h"

ISR(TIMER3_OVF_vect)
{
//	DDRD |= 1<<5;
//	PORTD ^= 1<<5;
}


static void (*Timer_Callback)() = 0;


ISR(TIMER3_COMPA_vect)
{
	Timer_Callback();
}


ISR(TIMER3_COMPB_vect)
{
//	DDRD |= 1<<5;
//	PORTD ^= 1<<5;
}

ISR(TIMER3_COMPC_vect)
{
//	DDRD |= 1<<5;
//	PORTD ^= 1<<5;
}


void Timer_Init()
{
	OCR3A  = 0x00;       
	 
	//disconnect OC1
	TCCR3A &= ~( (1<<COM3A1) | (1<<COM3A0) );
	
	// set speed
	TCCR3B &= ~( (1 << CS32) | (1 << CS31) | (1 << CS30) ); // clear old state

	// TCCR3B = (1 << CS30);  // clk / 1
	// TCCR3B = (1 << CS31);  // clk / 8
	// TCCR3B = (1 << CS31) | (1 << CS30);  // clk / 64
	// TCCR3B = (1 << CS32);  // clk / 256
	// TCCR3B = (1 << CS32) | (1 << CS30);  // clk / 1024

	/*
	//PWM, Phase Correct, 8-bit
	TCCR3A |= (1<<WGM30);
	TCCR3B |= 0;
	*/

	// OCR
	TCCR3A &= ~( (1<<WGM30) | (1<<WGM31) );
	TCCR3B |= (1<<WGM32);
	TCCR3B &= ~(1<<WGM33);
	
	Timer_SetCallback( 0 );

	TCNT3 = 0;

}

void Timer_SetSpeed( unsigned int v )
{
	OCR3A = v; 
}

//
// From 1mS to 4095 mS
// 
void Timer_SetMilliSeconds( unsigned int v )
{
	unsigned table[] = { 16000, 2000, 250,   62,   15 };
	unsigned top[]   = {     4,   32, 262, 1057, 4369 };

	for( char i=0;i<5;i++)
	{
		if ( v <= top[i] )
		{
			OCR3A = table[i] * v; 
			TCCR3B &= ~ ( ( 1 << CS30 ) | ( 1 << CS32 ) | ( 1 << CS31 ) );
			TCCR3B |= (i + 1) << CS30;
			return;
		}

	}	
}

//
// From 1uS to 4095 uS
// 
void Timer_SetMicroSeconds( unsigned int v )
{
	OCR3A = 16 * v; 
	TCCR3B &= ~ ( ( 1 << CS30 ) | ( 1 << CS32 ) | ( 1 << CS31 ) );
	TCCR3B |= (1 << CS30);
}


void Timer_SetCallback( void (*call)() )
{
	if ( call == 0 )
	{
		ETIMSK &= ~(1<<OCIE3A); //disable COMPA interrupt
	}
	else
	{
		ETIMSK |= (1<<OCIE3A); //enable COMPA interrupt
	}

	Timer_Callback = call;
}
